#include <navigation/navigation.h>

#include "math.h"

/**
 * Creates a Navigation instance.
 */
Navigation::Navigation(int argc, char **argv)
{
	ros::NodeHandle n;
	move_publisher = n.advertise<driver::Move>("/motor_controller/directions", 1, true);

	// Wait until we have a subscriber to directions topic.
	while (move_publisher.getNumSubscribers() < 1) {
		ros::spinOnce();
	}

	parse_arguments(argc, argv);
}

/**
 * Parses the arguments given on the command line into 
 * movement updates.
 **/
void Navigation::parse_arguments(int argc, char **argv) {
	// Parse arguments and send initial move request to robot
	float x = 0.0f;
	float y = 0.0f;
	float r = 0.0f;
	float d = 0.0f;
	int ch;
	while ((ch = getopt(argc, argv, "x:y:r:d:")) != -1) {
		switch (ch) {
			case 'x':
				x = atof(optarg);
				break;
			case 'y':
				y = atof(optarg);
				break;
			case 'r':
				r = atof(optarg);
				break;
			case 'd':
				d = atof(optarg);
				break;
		}
	}
	
	// Send a move message
	if (r != 0) {
		publish_move(r * (M_PI / 180.0f), 0.0f);
	} else if (d != 0) {
		publish_move(0.0f, d);
	} else if (x != 0 && y != 0) {
		publish_move(atan2f(y, x), sqrtf(y * y + x * x));
	}
}

void Navigation::publish_move(float angle, float distance) {
	driver::Move move;
	move.angle = angle;
	move.distance = distance;
	move_publisher.publish(move);
}

int main (int argc, char **argv)
{
  	ros::init(argc, argv, "navigation");
	new Navigation(argc, argv);
	return 0;
}
